#include "StdAfx.h"
#include "segment_2.h"
#include "box_2.h"
#include "boost\geometry\geometries\segment.hpp"
#include <boost\geometry\algorithms\intersects.hpp> 

double segment_2::distance(const point_2& point) const
{
  segment_2_ref ref_seg(*this);

  return ref_seg.distance(point);
}

double segment_2::distance2(const point_2& point) const
{
  segment_2_ref ref_seg(*this);

  return ref_seg.distance2(point);
}

bool segment_2::intersects(const box_2& box) const
{
  return start.intersects(box) || end.intersects(box);
}

bool segment_2::inside(const point_2& p) const
{
  if (start.x != end.x) {    // S is not  vertical
    if (start.x <= p.x && p.x <= end.x) {
      return true;
    }

    if (start.x >= p.x && p.x >= end.x) {
      return true;
    }
  } else {    // S is vertical, so test y  coordinate
    if (start.y <= p.y && p.y <= end.y) {
      return true;
    }

    if (start.y >= p.y && p.y >= end.y) {
      return true;
    }
  }

  return false;

}

int segment_2::intersects(const segment_2& s2, point_2& i0, point_2& i1) const
{
  const segment_2 &s1 = *this;
  vector_2    u = s1.end - s1.start;
  vector_2    v = s2.end - s2.start;
  vector_2    w = s1.start - s2.start;
  double      D = u|v;

  // test if  they are parallel (includes either being a point)
  if (fabs(D) < kSmallNum) {           // S1 and S2 are parallel
    if ((u|w) != 0 || (v|w) != 0) {
      return kDisjoint;                // they are NOT collinear
    }

    // they are collinear or degenerate
    // check if they are degenerate  points
    double du = u*u;
    double dv = v*v;
    if (du==0 && dv==0) {            // both segments are points
      if (s1.start !=  s2.start) {   // they are distinct  points
        return kDisjoint;
      }

      i0 = s1.start;                 // they are the same point
      return kUnique;
    }

    if (du==0) {                     // S1 is a single point
      if (s2.outside(s1.start)) {    // but is not in S2
        return kDisjoint;
      }

      i0 = s1.start;
      return kUnique;
    }

    if (dv==0) {                     // S2 a single point
      if (s1.outside(s2.start)) {    // but is not in S1
        return kDisjoint;
      }

      i0 = s2.start;
      return kUnique;
    }

    // they are collinear segments - get  overlap (or not)
    double t0=0, t1=0; // endpoints of S1 in eqn for S2
    vector_2 w2 = s1.end - s2.start;
    if (v.vx != 0) {
      t0 = w.vx / v.vx;
      t1 = w2.vx / v.vx;
    } else {
      t0 = w.vy / v.vy;
      t1 = w2.vy / v.vy;
    }

    if (t0 > t1) {                // must have t0 smaller than t1
      double t=t0; t0=t1; t1=t;   // swap if not
    }

    if (t0 > 1 || t1 < 0) {
      return kDisjoint;           // NO overlap
    }

    t0 = t0<0? 0 : t0;            // clip to min 0
    t1 = t1>1? 1 : t1;            // clip to max 1
    if (t0 == t1) {               // intersect is a point
      i0 = s2.start +  t0 * v;
      return kUnique;
    }

    // they overlap in a valid subsegment
    i0 = s2.start + t0 * v;
    i1 = s2.start + t1 * v;
    return kOverlap;
  }

  // the segments are skew and may intersect in a point
  // get the intersect parameter for S1
  double sI = v|w / D;
  if (sI < 0 || sI > 1) {  // no intersect with S1
    return kDisjoint;
  }

  // get the intersect parameter for S2
  double tI = u|w / D;
  if (tI < 0 || tI > 1) {  // no intersect with S2
    return kDisjoint;
  }

  i0 = s1.start + sI * u;  // compute S1 intersect point
  return kUnique;
}

double segment_2_ref::distance(const point_2& point) const
{
  vector_2 v = end-start;
  vector_2 w = point-start;
  double sw = w*v;
  if(sw<=0) {
    return w.length();
  }

  double sv = v*v;
  if(sv>0) {
    if(sv<=sw) {
      return point.distance(end);
    }

    point_2 ptb = start+sw/sv*v;
    return point.distance(ptb);
  } else {
    return w.length();
  }
}

double segment_2_ref::distance2(const point_2& point) const
{
  vector_2 v = end-start;
  vector_2 w = point-start;
  double sw = w*v;
  if(sw<=0) {
    return w.length2();
  }

  double sv = v*v;
  if(sv>0) {
    if(sv<=sw) {
      return point.distance2(end);
    }

    point_2 ptb = start+sw/sv*v;
    return point.distance2(ptb);
  } else {
    return w.length2();
  }
}
